/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2025 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "comp.h"
#include "cordic.h"
#include "dma.h"
#include "i2c.h"
#include "opamp.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "string.h"
#include "stdlib.h"
#include "segger_rtt_debug.h"

#include "atomic_delay.h"
#include "Foc.h"
#include "Svpwm.h"
#include "float2str.h"
#include "stspin32g4.h"
#include "motor.h"
#include "Pid.h"

// #include "atomic_usart.h"
// #include "atomic_sys.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define PWM_PERIOD 8500
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart1_tx;

volatile SysTick_Type *systick = SysTick;
uint8_t uart1_it_txe_status = 0;
uint8_t uart1_it_rxne_status = 0;
uint8_t uart1_it_idle_status = 0;
uint8_t uart_send_enable = 1;
Foc foc = {
    .Init = Foc_Init,
    .param_clark_mode = CLARK_MODE_EQUAL_AMPLITUDE_TRANSFORM};
Svpwm svpwm = {
    .Init = Svpwm_Init,
    .param_Ts = 1.0F,
    .param_Udc = 1.0F,
};
Stspin32g4 stspin32g4 = {
    .Init = Stspin32g4_Init,
    .param_i2c_addr = STSPIN32G4_I2C_ADDR,
    .param_reg_config = {
        .POWMNG = {
            .VCC_VAL = 3,
            .RESERVE1 = 0,
            .STBY_REG_EN = 0,
            .VCC_DIS = 0,
            .REG3V3_DIS = 0,
            .RESERVE2 = 0,
        },
        .LOGIC = {
            .ILOCK = 1,
            .DTMIN = 1,
            .VDS_P_DFG = 0,
            .RESERVE = 0x07,
        },
        .READY = {
            .VCC_UVLO_RDY = 1,
            .THSD_RDY = 0,
            .RESERVE1 = 0,
            .STBY_RDY = 1,
            .RESERVE2 = 0,
        },
        .NFAULT = {
            .VCC_UVLO_FLT = 1,
            .THSD_FLT = 1,
            .VDS_P_FLT = 1,
            .RESERVE = 0x0f,
        },
    },
};
Motor motor = {
    .param_R = 1.0F,
    .param_L = 1.0e-3F,
    .param_amp_gain = 11.0F / 1.5F,
    .param_R_sample = 0.01F,
};
Pid id_pid = {
    .Init = Pid_Init,
    .param_en = 1,
    .param_bypass = 0,
    .param_T = 50e-6F,
    .param_pid_mode = PID_MODE_KP_KI_INDEPENDENT,
    .param_kp = 0.3F,
    .param_ki = 100.0F,
    .param_kd = 0.0F,
    .param_kc = 0.0F,
    .param_limit_ref = 5.0F,
    .param_limit_ui = 0.5F,
    .parma_limit_out = 0.5F,
};
Pid iq_pid = {
    .Init = Pid_Init,
    .param_en = 1,
    .param_bypass = 0,
    .param_T = 50e-6F,
    .param_pid_mode = PID_MODE_KP_KI_INDEPENDENT,
    .param_kp = 0.3F,
    .param_ki = 100.0F,
    .param_kd = 0.0F,
    .param_kc = 0.0F,
    .param_limit_ref = 5.0F,
    .param_limit_ui = 0.5F,
    .parma_limit_out = 0.5F,
};
uint8_t gpio_read_val_wake = 0;
uint8_t gpio_read_val_ready = 0;
uint8_t gpio_read_val_nfault = 0;
uint8_t gpio_write_val_BZ = 0;
uint32_t mainloop_tick = 0;
uint32_t isr_tick = 0;

uint8_t uart_str_send[] = "hi get!";
uint8_t uart_str_receive[100] = {};

char cmd_and_arg[5][50] = {};

float theta_step = 0.0F;

uint16_t adc_a = 0;
uint16_t adc_b = 0;

#define printfX(...)                                                               \
    {                                                                              \
        HAL_GPIO_WritePin(UART1_DE_RE_GPIO_Port, UART1_DE_RE_Pin, GPIO_PIN_SET);   \
        printf(__VA_ARGS__);                                                       \
        HAL_GPIO_WritePin(UART1_DE_RE_GPIO_Port, UART1_DE_RE_Pin, GPIO_PIN_RESET); \
    }
/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{

    /* USER CODE BEGIN 1 */

    /* USER CODE END 1 */

    /* MCU Configuration--------------------------------------------------------*/

    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
    HAL_Init();

    /* USER CODE BEGIN Init */
    delay_init(170);

    // foc.ud = 0.1F;

    /* USER CODE END Init */

    /* Configure the system clock */
    SystemClock_Config();

    /* USER CODE BEGIN SysInit */

    /* USER CODE END SysInit */

    /* Initialize all configured peripherals */
    MX_GPIO_Init();
    MX_DMA_Init();
    MX_USART1_UART_Init();
    MX_TIM1_Init();
    MX_I2C3_Init();
    MX_ADC1_Init();
    MX_ADC2_Init();
    MX_COMP1_Init();
    MX_COMP2_Init();
    MX_COMP4_Init();
    MX_CORDIC_Init();
    MX_OPAMP1_Init();
    MX_OPAMP2_Init();
    /* USER CODE BEGIN 2 */

    // HAL_GPIO_WritePin(BZ_GPIO_Port, BZ_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(UART1_DE_RE_GPIO_Port, UART1_DE_RE_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(WAKE_GPIO_Port, WAKE_Pin, GPIO_PIN_RESET);
    HAL_UARTEx_ReceiveToIdle_DMA(&huart1, uart_str_receive, sizeof(uart_str_receive));
    __HAL_DMA_DISABLE_IT(&hdma_usart1_rx, DMA_IT_HT);

    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
    __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, PWM_PERIOD);
    __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, PWM_PERIOD);
    __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, PWM_PERIOD);

    HAL_TIM_OC_Start(&htim1, TIM_CHANNEL_4);

    HAL_OPAMP_Start(&hopamp1);
    HAL_OPAMP_Start(&hopamp2);

    motor.adc_init_ok = 0;
    HAL_TIM_Base_Start_IT(&htim1);

    foc.Init(&foc);
    svpwm.Init(&svpwm);
    stspin32g4.Init(&stspin32g4);
    id_pid.Init(&id_pid);
    iq_pid.Init(&iq_pid);
    id_pid.in_ref = 0.0F;
    iq_pid.in_ref = 0.0F;

    delay_ms(100);

    /* adc calibration */
    HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
    HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);
    delay_ms(100);
    HAL_ADCEx_InjectedStart(&hadc1);
    HAL_ADCEx_InjectedStart(&hadc2);

    for (uint16_t i = 0; i < 100; i++)
    {
        HAL_ADC_PollForConversion(&hadc1, 10);
        motor.adc_a_sum += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
        HAL_ADC_PollForConversion(&hadc2, 10);
        motor.adc_b_sum += HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);
        delay_ms(1);
    }
    motor.Ia_adc_offset = (int16_t)(motor.adc_a_sum / 100);
    motor.Ib_adc_offset = (int16_t)(motor.adc_b_sum / 100);

    motor.adc_init_ok = 1;

    /* USER CODE END 2 */

    /* Infinite loop */
    /* USER CODE BEGIN WHILE */
    while (1)
    {
        //	HAL_UART_Transmit(&huart1, "hello\r\n", 7, 500);
        //    HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_15);

        // printfX("mainloop: %ld\r\n", mainloop_tick);

        // HAL_ADC_Start(&hadc1);
        // HAL_ADC_PollForConversion(&hadc1, 100);
        // HAL_ADC_Start(&hadc2);
        // HAL_ADC_PollForConversion(&hadc2, 100);
        // delay_ms(500);

        //	printfX("test:%d\r\n", systick->VAL);
        //	if (uart_send_enable)
        //	{
        gpio_read_val_wake = HAL_GPIO_ReadPin(WAKE_GPIO_Port, WAKE_Pin);
        gpio_read_val_ready = HAL_GPIO_ReadPin(READY_GPIO_Port, READY_Pin);
        gpio_read_val_nfault = HAL_GPIO_ReadPin(nFAULT_GPIO_Port, nFAULT_Pin);
        //
        //	    HAL_GPIO_WritePin(BZ_GPIO_Port, BZ_Pin, gpio_write_val_BZ);
        //		HAL_GPIO_WritePin(UART1_DE_RE_GPIO_Port, UART1_DE_RE_Pin, GPIO_PIN_SET);
        //		printfX("-----\r\n");
        //		printfX("isr_tick: %ld\r\n", isr_tick);
        ////		printfX("wake: %d\r\n", gpio_read_val_wake);
        ////		printfX("ready: %d\r\n", gpio_read_val_ready);
        ////		printfX("nfault: %d\r\n", gpio_read_val_nfault);
        ////		printfX("uart1_it_txe_status: %d\r\n", uart1_it_txe_status);
        ////		printfX("uart1_it_rxne_status: %d\r\n", uart1_it_rxne_status);
        ////		printfX("uart1_it_idle_status: %d\r\n", uart1_it_idle_status);
        //		HAL_GPIO_WritePin(UART1_DE_RE_GPIO_Port, UART1_DE_RE_Pin, GPIO_PIN_RESET);
        //	}

        //    HAL_GPIO_TogglePin(HALL1_GPIO_Port, HALL1_Pin);
        //    HAL_GPIO_TogglePin(HALL2_GPIO_Port, HALL2_Pin);
        //    HAL_GPIO_TogglePin(HALL3_GPIO_Port, HALL3_Pin);

        mainloop_tick++;
        //	for (int i = 0;  i < 100; ++ i) {
        //		for (int j = 0;  j < 100; ++ j) {
        //			__NOP();
        //		}
        //	}

        /* USER CODE END WHILE */

        /* USER CODE BEGIN 3 */
    }
    /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    /** Configure the main internal regulator output voltage
     */
    HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);

    /** Initializes the RCC Oscillators according to the specified parameters
     * in the RCC_OscInitTypeDef structure.
     */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
    RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV4;
    RCC_OscInitStruct.PLL.PLLN = 85;
    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
    RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
    RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /** Initializes the CPU, AHB and APB buses clocks
     */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
    {
        Error_Handler();
    }
}

/* USER CODE BEGIN 4 */

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    // HAL_GPIO_WritePin(HALL1_GPIO_Port, HALL1_Pin, GPIO_PIN_SET);
    if (htim == (&htim1))
    {
        if (motor.adc_init_ok == 1)
        {
            motor.Ia_adc_raw = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
            motor.Ib_adc_raw = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);
            motor.Ia_adc = motor.Ia_adc_raw - motor.Ia_adc_offset;
            motor.Ib_adc = motor.Ib_adc_raw - motor.Ib_adc_offset;

            motor.Ia = -motor.Ia_adc / 4096.0F * 3.3 / motor.param_amp_gain / motor.param_R_sample;
            motor.Ib = -motor.Ib_adc / 4096.0F * 3.3 / motor.param_amp_gain / motor.param_R_sample;
            motor.Ic = -motor.Ia - motor.Ib;

            foc.ia = motor.Ia;
            foc.ib = motor.Ib;
            foc.ic = motor.Ic;
            foc.Clark(foc.ia, foc.ib, foc.ic, &foc.ialpha, &foc.ibeta);
            foc.Park(foc.ialpha, foc.ibeta, foc.theta, &foc.id, &foc.iq);

            // id_pid.in_ref = 0;
            id_pid.in_fdb = foc.id;
            id_pid.Run(&id_pid);
            foc.ud = id_pid.out;

            // iq_pid.in_ref = 0;
            iq_pid.in_fdb = foc.iq;
            iq_pid.Run(&iq_pid);
            foc.uq = iq_pid.out;

            if (motor.en == 1)
            {
                foc.theta = fmodf(foc.theta + theta_step, 2 * M_PI);
                foc.IPark(foc.ud, foc.uq, foc.theta, &foc.ualpha, &foc.ubeta);

                svpwm.in_ualpha = foc.ualpha;
                svpwm.in_ubeta = foc.ubeta;
                svpwm.Run(&svpwm);
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, (uint16_t)(svpwm.out_Tcm1 * PWM_PERIOD));
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, (uint16_t)(svpwm.out_Tcm2 * PWM_PERIOD));
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, (uint16_t)(svpwm.out_Tcm3 * PWM_PERIOD));
            }
            else
            {
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, (uint16_t)(PWM_PERIOD));
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, (uint16_t)(PWM_PERIOD));
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, (uint16_t)(PWM_PERIOD));
            }

            // Log_Data();
        }
        isr_tick++;
        // HAL_GPIO_WritePin(HALL1_GPIO_Port, HALL1_Pin, GPIO_PIN_RESET);
    }
}

void cmd_analysis(uint8_t *data_buff, uint16_t data_len)
{
    uint8_t i;
    uint8_t arg_num = 0;
    uint8_t arg_str_i = 0;
    uint16_t len = 0;

    //    printfX("data_buff");
    len = data_len; // strlen(uart_str_receive);

    for (i = 0; i < len; i++)
    {
        if (data_buff[i] != ' ')
        {
            cmd_and_arg[arg_num][arg_str_i] = data_buff[i];
            arg_str_i++;
        }
        else
        {
            cmd_and_arg[arg_num][arg_str_i] = '\0';
            arg_num++;
            arg_str_i = 0;
        }
    }
    cmd_and_arg[arg_num][arg_str_i] = '\0';

    /* 执行命令 */
    // for (size_t i = 0; i < self->cmd_nums; i++)
    // {
    //     if (strcmp((const char *)cmd_and_arg[0], self->cmd_table[i].name) == 0)
    //     {
    //         self->cmd_table[i].func(self, cmd_and_arg);
    //         break;
    //     }
    // }

    /* printfX cmd */
    // printfX("cmd: ");
    // for ( i = 0; i <= arg_num; i++)
    // {
    //     printfX("%s ", cmd_and_arg[i]);
    // }
    // printfX("\r\n");

    /* cmd parser */
    if (strcmp((const char *)cmd_and_arg[0], "motor") == 0)
    {

        if (strcmp((const char *)cmd_and_arg[1], "en") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "toggle") == 0)
            {
                motor.en = !motor.en;
                printfX("motor en: %d\r\n", motor.en);
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "set") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "Id") == 0)
            {
                id_pid.in_ref = atof((const char *)cmd_and_arg[3]);
                printfX("id ref: %s\r\n", float2str(id_pid.in_ref, 2));
            }
            else if (strcmp((const char *)cmd_and_arg[2], "speed_rpm") == 0)
            {
                motor.speed_rpm = atof((const char *)cmd_and_arg[3]);
                theta_step = motor.speed_rpm / 60 * 2 * M_PI * 50e-6F;
                printfX("speed_rpm_ref: %s\r\n", float2str(motor.speed_rpm, 2));
                printfX("theta_step: %s\r\n", float2str(theta_step, 4));
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "status") == 0)
        {
            printfX("motor_en: %d\r\n", motor.en);
            printfX("speed_rpm_ref: %s\r\n", float2str(motor.speed_rpm, 2));
            printfX("id_ref: %s | id_fdb: %s\r\n", float2str(id_pid.in_ref, 2), float2str(id_pid.in_fdb, 2));
        }
    }
    else if (strcmp((const char *)cmd_and_arg[0], "pwm") == 0)
    {

        if (strcmp((const char *)cmd_and_arg[1], "set") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "a") == 0)
            {
                float duty = atof((const char *)cmd_and_arg[3]);
                uint16_t cmp = (uint16_t)(duty * PWM_PERIOD);
                printfX("pwm duty a: %s %d\r\n", float2str(duty, 2), cmp);
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, cmp);
            }
            else if (strcmp((const char *)cmd_and_arg[2], "b") == 0)
            {
                float duty = atof((const char *)cmd_and_arg[3]);
                uint16_t cmp = (uint16_t)(duty * PWM_PERIOD);
                printfX("pwm duty b: %s %d\r\n", float2str(duty, 2), cmp);
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, cmp);
            }
            else if (strcmp((const char *)cmd_and_arg[2], "c") == 0)
            {
                float duty = atof((const char *)cmd_and_arg[3]);
                uint16_t cmp = (uint16_t)(duty * PWM_PERIOD);
                printfX("pwm duty c: %s %d\r\n", float2str(duty, 2), cmp);
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, cmp);
            }
            else
            {
                float duty_a = atof((const char *)cmd_and_arg[3]);
                float duty_b = atof((const char *)cmd_and_arg[4]);
                float duty_c = atof((const char *)cmd_and_arg[5]);
                uint16_t cmp_a = (uint16_t)(duty_a * PWM_PERIOD);
                uint16_t cmp_b = (uint16_t)(duty_b * PWM_PERIOD);
                uint16_t cmp_c = (uint16_t)(duty_c * PWM_PERIOD);
                printfX("pwm duty abc: %s %s %s\r\n", float2str(duty_a, 2),
                        float2str(duty_b, 2),
                        float2str(duty_c, 2));
                printfX("pwm cmp abc: %d %d %d\r\n", cmp_a, cmp_b, cmp_c);
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, cmp_a);
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, cmp_b);
                __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, cmp_c);
            }
        }

        else if (strcmp((const char *)cmd_and_arg[1], "get") == 0)
        {
            // if (strcmp((const char *)cmd_and_arg[2], "a") == 0)
            // {
            //     float duty = atof((const char *)cmd_and_arg[3]);
            //     uint16_t cmp = (uint16_t)(duty * PWM_PERIOD);
            //     printfX("pwm duty a: %s %d\r\n", float2str(duty, 2), cmp);
            //     __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, cmp);
            // }
            // else if (strcmp((const char *)cmd_and_arg[2], "b") == 0)
            // {
            //     float duty = atof((const char *)cmd_and_arg[3]);
            //     uint16_t cmp = (uint16_t)(duty * PWM_PERIOD);
            //     printfX("pwm duty b: %s %d\r\n", float2str(duty, 2), cmp);
            //     __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, cmp);
            // }
            // else if (strcmp((const char *)cmd_and_arg[2], "c") == 0)
            // {
            //     float duty = atof((const char *)cmd_and_arg[3]);
            //     uint16_t cmp = (uint16_t)(duty * PWM_PERIOD);
            //     printfX("pwm duty c: %s %d\r\n", float2str(duty, 2), cmp);
            //     __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, cmp);
            // }
            // else
            // {

            //     printfX("pwm duty abc: %s %s %s\r\n", float2str(duty_a, 2),
            //            float2str(duty_b, 2),
            //            float2str(duty_c, 2));
            //     printfX("pwm cmp abc: %d %d %d\r\n", cmp_a, cmp_b, cmp_c);
            // }
        }
    }
    else if (strcmp((const char *)cmd_and_arg[0], "foc") == 0)
    {
        if (strcmp((const char *)cmd_and_arg[1], "set") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "ud") == 0)
            {
                foc.ud = atof((const char *)cmd_and_arg[3]);
            }
            else if (strcmp((const char *)cmd_and_arg[2], "uq") == 0)
            {
                foc.uq = atof((const char *)cmd_and_arg[3]);
            }
            else if (strcmp((const char *)cmd_and_arg[2], "theta") == 0)
            {
                foc.theta = atof((const char *)cmd_and_arg[3]);
            }
            printfX("foc.ud: %s\r\n", float2str(foc.ud, 2));
            printfX("foc.uq: %s\r\n", float2str(foc.uq, 2));
            printfX("foc.theta: %s\r\n", float2str(foc.theta, 2));
        }
    }
    else if (strcmp((const char *)cmd_and_arg[0], "stspin32g4") == 0)
    {
        if (strcmp((const char *)cmd_and_arg[1], "read") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "all") == 0)
            {
                stspin32g4.ReadAllReg(&stspin32g4);
                printfX("stspin32g4 read all reg ok!\r\n");
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "config") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "all") == 0)
            {
                stspin32g4.Cmd_LockControl(&stspin32g4, 0);
                stspin32g4.WriteConfig(&stspin32g4);
                stspin32g4.Cmd_LockControl(&stspin32g4, 1);
                stspin32g4.ReadAllReg(&stspin32g4);
                printfX("stspin32g4 config all reg ok!\r\n");
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "reset") == 0)
        {
            stspin32g4.Cmd_LockControl(&stspin32g4, 0);
            stspin32g4.Cmd_Reset(&stspin32g4);
            stspin32g4.Cmd_LockControl(&stspin32g4, 1);
            stspin32g4.ReadAllReg(&stspin32g4);
            printfX("stspin32g4 reset!\r\n");
        }
        else if (strcmp((const char *)cmd_and_arg[1], "low_consumption_mode") == 0)
        {
            uint8_t val = (uint8_t)atoi((const char *)cmd_and_arg[2]);
            stspin32g4.Cmd_LockControl(&stspin32g4, 0);
            stspin32g4.Cmd_LowConsumptionMode(&stspin32g4, val);
            stspin32g4.Cmd_LockControl(&stspin32g4, 1);
            stspin32g4.ReadAllReg(&stspin32g4);
            if (val)
            {
                printfX("stspin32g4 enter low consumption mode!\r\n");
            }
            else
            {
                printfX("stspin32g4 exit low consumption mode!\r\n");
            }
        }
        else if (strcmp((const char *)cmd_and_arg[1], "fault_clear") == 0)
        {
            stspin32g4.Cmd_LockControl(&stspin32g4, 0);
            stspin32g4.Cmd_FaultClear(&stspin32g4);
            stspin32g4.Cmd_LockControl(&stspin32g4, 1);
            stspin32g4.ReadAllReg(&stspin32g4);
            printfX("stspin32g4 fault clear!\r\n");
        }
        else if (strcmp((const char *)cmd_and_arg[1], "lock_control") == 0)
        {
            uint8_t val = (uint8_t)atoi((const char *)cmd_and_arg[2]);
            stspin32g4.Cmd_LockControl(&stspin32g4, val);
            stspin32g4.ReadAllReg(&stspin32g4);
            if (val)
            {
                printfX("stspin32g4 lock!\r\n");
            }
            else
            {
                printfX("stspin32g4 unlock!\r\n");
            }
        }
    }
    else if (strcmp((const char *)cmd_and_arg[0], "pid") == 0)
    {
        if (strcmp((const char *)cmd_and_arg[1], "set") == 0)
        {
            if (strcmp((const char *)cmd_and_arg[2], "kp") == 0)
            {
                id_pid.kp = (float)atof(cmd_and_arg[3]);
                iq_pid.kp = (float)atof(cmd_and_arg[3]);
            }
            else if (strcmp((const char *)cmd_and_arg[2], "ki") == 0)
            {
                id_pid.ki = (float)atof(cmd_and_arg[3]);
                iq_pid.ki = (float)atof(cmd_and_arg[3]);
            }
            printfX("kp: %s\r\n", float2str(id_pid.kp, 2));
            printfX("ki: %s\r\n", float2str(id_pid.ki, 2));
        }
        else if (strcmp((const char *)cmd_and_arg[1], "get") == 0)
        {
            printfX("kp: %s\r\n", float2str(id_pid.kp, 2));
            printfX("ki: %s\r\n", float2str(id_pid.ki, 2));
        }
    }

    //  __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, 4200);
}

void HAL_UARTEx_RxFifoFullCallback(UART_HandleTypeDef *huart)
{
    uart_send_enable = 1;
}

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    /* Set transmission flag: transfer complete */
    uart_send_enable = 1;
    //	printfX("2\r\n");

    if (huart == &huart1)
    {
        // HAL_UART_Transmit_DMA(huart, uart_str_receive, sizeof(uart_str_receive));

        cmd_analysis(uart_str_receive, Size);

        HAL_UARTEx_ReceiveToIdle_DMA(&huart1, uart_str_receive, sizeof(uart_str_receive));
        __HAL_DMA_DISABLE_IT(&hdma_usart1_rx, DMA_IT_HT);
    }
}

/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
    /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1)
    {
    }
    /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
    /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printfX("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
